當我有了影像特徵點的深度資訊之後，
我們就要來回復第一組深度影像與第二組深度影像的運動資訊
(每組深度影像是由兩張平面影像透過視差的關係組合而成)，
我們可以透過第一組的左邊相機的影像與第二組左邊相機的影像，
進行特徵點偵測與匹配，從而得出兩群3D點之間的匹配關係。

接下來的問題就會是相機的剛體轉換問題，
這邊就讓我介紹由K. S. Arun等人(1987)提出一種解法並由Eigen的實作

1. 先計算兩群3D點的中心點

cv::Point3d p1, p2;
size_t N = points1.size();
for (int i = 0; i < N; i++)
{
    p1 += points1[i];
    p2 += points2[i];
}
p1 = cv::Point3d(cv::Vec3d(p1) / (int)N);
p2 = cv::Point3d(cv::Vec3d(p2) / (int)N);

2.將兩群3D點平移使中心點平移到圓心

std::vector<cv::Point3d> q1(N), q2(N);
for (int i = 0; i < N; i++)
{
    q1[i] = points1[i] - p1;
    q2[i] = points2[i] - p2;
}

3.計算相關矩陣

Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for (int i = 0; i < N; i++) {
    W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z)*Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
}

4.SVD分解相關矩陣

Eigen::JacobiSVD<Eigen::Matrix3d>svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();

5.組合旋轉矩陣R

Eigen::Matrix3d R_ = V * (U.transpose());//so this rotation is rotate A to B

6.透過中心點求解 位移t

Eigen::Vector3d t_ = Eigen::Vector3d(p2.x, p2.y, p2.z) - R_ * Eigen::Vector3d(p1.x, p1.y, p1.z);

類似的問題在數學正交 Procrustes 問題有興趣的朋友可以深入了解，相關矩陣的數學意義........